#ifndef UHM_MAT_LINAL_MAP_HXX_
#define UHM_MAT_LINAL_MAP_HXX_

#define UHM_MAT_LINAL_MAP_UNROLL 4

namespace uhm {

  template<class T_> void op_s_copy(T_& a, T_& b) { b  = a; }
  template<class T_> void op_s_axpy(T_& a, T_& b) { b += a; }

  template<class T_> void op_s_reverse_copy(T_& a, T_& b) { a  = b; }
  template<class T_> void op_s_reverse_axpy(T_& a, T_& b) { a += b; }

  template<class T_,class I_> 
  inline void op_s(Mapper_<I_> &M_col, Mapper_<I_> &M_row,
                   Mat_Linal_ &S, Mat_Linal_ &T,
                   void (*op)(T_&, T_&) ) {
    linal::Flat_ &SS = S.flat(), &TT = T.flat();

    void *buffer_SS = SS.get_buffer();
    void *buffer_TT = TT.get_buffer();

    Int_ cs_SS = SS.get_cs();
    Int_ cs_TT = TT.get_cs();

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
    for (Int_ k2=0;k2<M_row.in()->get_size();++k2) {
      if (M_row.idx(k2) < 0)
        continue; // -1 is not mapped

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
      for (Int_ k1=0;k1<M_col.in()->get_size();++k1) {
        if (M_col.idx(k1) < 0) {
          continue; // -1 is not mapped
        } else {
          op(linal::elem<T_>( buffer_SS, cs_SS, 1,
                              M_col.in()->idx(k1),
                              M_row.in()->idx(k2) ) ,
             linal::elem<T_>( buffer_TT, cs_TT, 1,
                              M_col.out()->idx(M_col.idx(k1)),
                              M_row.out()->idx(M_row.idx(k2)) ));
        }
      }
    }
  }

  template<class T_,class I_> 
  inline void op_s(Mapper_<I_> &M_col, 
                   Mat_Linal_ &S, Mat_Linal_ &T,
                   void (*op)(T_&, T_&) ) {
    linal::Flat_ &SS = S.flat(), &TT = T.flat();

    void *buffer_SS = SS.get_buffer();
    void *buffer_TT = TT.get_buffer();

    Int_ cs_SS = SS.get_cs();
    Int_ cs_TT = TT.get_cs();

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
    for (Int_ k2=0;k2<SS.get_n();++k2) {

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
      for (Int_ k1=0;k1<M_col.in()->get_size();++k1) {
        if (M_col.idx(k1) < 0)
          continue; // -1 is not mapped
        else 
          op(linal::elem<T_>( buffer_SS, cs_SS, 1,
                              M_col.in()->idx(k1),
                              k2 ) ,
             linal::elem<T_>( buffer_TT, cs_TT, 1,
                              M_col.out()->idx(M_col.idx(k1)),
                              k2 ));
      }
    }
  }

  inline void op_b_copy(FLA_Obj &S, FLA_Obj &T) { FLA_Copy( S, T ); }
  inline void op_b_axpy(FLA_Obj &S, FLA_Obj &T) { FLA_Axpy( FLA_ONE, S, T); }

  inline void op_b_reverse_copy(FLA_Obj &S, FLA_Obj &T) { FLA_Copy( T, S ); }
  inline void op_b_reverse_axpy(FLA_Obj &S, FLA_Obj &T) { FLA_Axpy( FLA_ONE, T, S); }

  template<class I_> 
  inline void op_b(Mapper_<I_> &M_col, Mapper_<I_> &M_row,
                   Mat_Linal_ &S, Mat_Linal_ &T,
                   void (*op)(FLA_Obj &, FLA_Obj &) ) {
    linal::Flat_ &SS = S.flat(), &TT = T.flat();
    linal::Flat_ blk_in, blk_out;

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
    for (Int_ k2=0;k2<M_row.in()->get_size();++k2) {
      if (M_row.idx(k2) <0)
        continue;

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
      for (Int_ k1=0;k1<M_col.in()->get_size();++k1) {
        if (M_col.idx(k1) < 0) {
          continue;
        } else {
          SS.extract( blk_in, 
                      M_col.in()->id(k1).dof(), 
                      M_row.in()->id(k2).dof(),
                      M_col.in()->idx(k1), 
                      M_row.in()->idx(k2) );
          TT.extract( blk_out,
                      M_col.out()->id(M_col.idx(k1)).dof(), 
                      M_row.out()->id(M_row.idx(k2)).dof(),
                      M_col.out()->idx(M_col.idx(k1)),
                      M_row.out()->idx(M_row.idx(k2)) );
          op( ~blk_in, ~blk_out );
        }
      }
    }
  }

  template<class I_> 
  inline void op_b(Mapper_<I_> &M_col, Mat_Linal_ &S, Mat_Linal_ &T,
                   void (*op)(FLA_Obj &, FLA_Obj &) ) {
    linal::Flat_ &SS = S.flat(), &TT = T.flat();
    linal::Flat_ blk_in, blk_out;

#pragma unroll(UHM_MAT_LINAL_MAP_UNROLL)
    for (Int_ k1=0;k1<M_col.in()->get_size();++k1) {
      if (M_col.idx(k1) < 0) {
        continue;
      } else {
        SS.extract( blk_in, 
                    M_col.in()->id(k1).dof(), 
                    SS.get_n(),
                    M_col.in()->idx(k1), 
                    0 );
        TT.extract( blk_out,
                    M_col.out()->id(M_col.idx(k1)).dof(), 
                    SS.get_n(),
                    M_col.out()->idx(M_col.idx(k1)),
                    0 );
        op( ~blk_in, ~blk_out );
      }
    }
  }


  // Forward
  // -------
  template<class I_> 
  inline void copy(Mapper_<I_> &M_col, Mapper_<I_> &M_row,
                   Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,M_row,S,T,op_s_copy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,M_row,S,T,op_s_copy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,M_row,S,T,op_s_copy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,M_row,S,T,op_s_copy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,M_row,S,T,op_b_copy);
      break;
    }
    }
  }
  
  template<class I_> 
  inline void axpy(Mapper_<I_> &M_col, Mapper_<I_> &M_row,
                   Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,M_row,S,T,op_s_axpy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,M_row,S,T,op_s_axpy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,M_row,S,T,op_s_axpy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,M_row,S,T,op_s_axpy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,M_row,S,T,op_b_axpy);
      break;
    }
    }
  }

  template<class I_> 
  inline void copy(Mapper_<I_> &M_col, Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,S,T,op_s_copy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,S,T,op_s_copy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,S,T,op_s_copy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,S,T,op_s_copy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,S,T,op_b_copy);
      break;
    }
    }
  }
  
  template<class I_> 
  inline void axpy(Mapper_<I_> &M_col, Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,S,T,op_s_axpy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,S,T,op_s_axpy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,S,T,op_s_axpy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,S,T,op_s_axpy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,S,T,op_b_axpy);
      break;
    }
    }
  }

  // Reverse
  // -------
  template<class I_> 
  inline void copy_reverse(Mapper_<I_> &M_col, Mapper_<I_> &M_row,
                           Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,M_row,S,T,op_s_reverse_copy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,M_row,S,T,op_s_reverse_copy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,M_row,S,T,op_s_reverse_copy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,M_row,S,T,op_s_reverse_copy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,M_row,S,T,op_b_reverse_copy);
      break;
    }
    }
  }
  
  template<class I_> 
  inline void axpy_reverse(Mapper_<I_> &M_col, Mapper_<I_> &M_row,
                           Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,M_row,S,T,op_s_reverse_axpy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,M_row,S,T,op_s_reverse_axpy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,M_row,S,T,op_s_reverse_axpy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,M_row,S,T,op_s_reverse_axpy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,M_row,S,T,op_b_reverse_axpy);
      break;
    }
    }
  }

  template<class I_> 
  inline void copy_reverse(Mapper_<I_> &M_col, Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,S,T,op_s_reverse_copy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,S,T,op_s_reverse_copy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,S,T,op_s_reverse_copy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,S,T,op_s_reverse_copy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,S,T,op_b_reverse_copy);
      break;
    }
    }
  }
  
  template<class I_> 
  inline void axpy_reverse(Mapper_<I_> &M_col, Mat_Linal_ &S, Mat_Linal_ &T) {
    switch(I_::_Unit) {
    case I_::SCALAR: {
      switch(T.get_type()) {
      case Mat_Base_::FLOAT : op_s<Float_, I_>(M_col,S,T,op_s_reverse_axpy);break;
      case Mat_Base_::DOUBLE: op_s<Double_,I_>(M_col,S,T,op_s_reverse_axpy);break;
      case Mat_Base_::FCMPLX: op_s<fCmplx_,I_>(M_col,S,T,op_s_reverse_axpy);break;
      case Mat_Base_::DCMPLX: op_s<dCmplx_,I_>(M_col,S,T,op_s_reverse_axpy);break;
      }
      break;
    }
    case I_::BLOCK: {
      op_b<I_>(M_col,S,T,op_b_reverse_axpy);
      break;
    }
    }
  }

}

#endif
